#ifndef _POSERESOLVE_H
#define _POSERESOLVE_H


#include <opencv2/opencv.hpp>

using namespace cv;

using namespace std;

//using Eigen3
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <complex>
#include <math.h>
#include <cfloat>
#include<limits.h>  //int max

//DJI_CAM HEAD
#include <stdio.h>
#include <setjmp.h>
#include <stdlib.h>
#include <malloc.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <getopt.h>
#include <pthread.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <ctype.h>


#define SHOW_IMAGE               0  //define if show image or draw image
#define SAVE_IMAGE               0
#define FRAME_SIZE              (1280*720*3/2)  /*format NV12*/
#define BLOCK_MODE                     1    // 阻塞模式设置

// 大疆摄像头使用test里面　定义的参数　用于启动时设置
#define DISPLAY_MODE            (1 << 0)    // 第一位　电脑(linux)显示视频
#define GETBUFFER_MODE          (1 << 1)    // 第二位　将视频存入sd卡
#define TRANSFER_MODE           (1 << 2)    // 第三位　手机显示视频(图像通过飞控传到遥控器，在让手机显示)

#define CAM_BLOCK                1
#define CAM_NON_BLOCK            0
#define PI 3.141593

struct CamPos
{
    double Position_X;
    double Position_Y;
    double Position_Z;
    double Distance;
    double roll;
    double pitch;
    double yaw;

    Point2f PosMcircle;  //position of tracked point
    bool tar_detec_flag; //0:not detect
    bool gimbal_mode;    //0:search mode,1:tract mode
};


void cal_camera_position(cv::Point2f *feature_points, cv::Mat rvecsMat, cv::Mat tvecsMat, cv::Mat Cam_position);
void push_feature_points_position(int id);


void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll); // 旋转矩阵　转到　yaw pitch roll

#endif
